/** \file
 * \brief Example code for Simple Open EtherCAT master
 *
 * Usage : simple_test [ifname1]
 * ifname is NIC interface, f.e. eth0
 *
 * This is a minimal test.
 *
 * (c)Arthur Ketels 2010 - 2011
 */

 #include <stdio.h>
 #include <string.h>
 #include <inttypes.h>
 #include <unistd.h>
 #include <signal.h>
 #include <time.h>

 #include "ethercat.h"
 #include "main.h"
 
 #define DC_ENABLE 1
 #define DC_SYNC0 1000
 #define DC_SYNC1 1000
 #define EC_TIMEOUTMON 500
 
 char IOmap[4096];
 OSAL_THREAD_HANDLE osal_ecatcheck_thread;
 OSAL_THREAD_HANDLE osal_ecatdata_thread;
 OSAL_THREAD_HANDLE osal_ecattime_thread;
 int expectedWKC;
 volatile int wkc;
 boolean inOP;
 boolean needlf;
 boolean ecat_signal=FALSE;
 boolean config_ok = FALSE;
 uint8 currentgroup = 0;

 ////////////
 output_t *ecat_output[2];
 input_t *ecat_input[2];
 /////////////
 #define DELAY 800
static int delay = 0;
static int delay_offset = 0;
static int slaves_offset_cnt[10]; //保存每个从站的延时矫正次数

static unsigned long long total_cycle=0;
static uint32 slave_data[2];
static uint32 slave_data_old[2];
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

#define ARRAYLEN(x) sizeof(x)/sizeof(x[0])

OSAL_THREAD_FUNC ecat_data( void *ptr )
{
    while(ecat_signal == FALSE && config_ok == TRUE)
    {
        // 1. 装载数据
        for (int slaves = 0;slaves<ec_slavecount;slaves++)
        {
            for (int i=0;i<ARRAYLEN(ecat_output[slaves]->channel);i++)
            {
                ecat_output[slaves]->channel[i] = total_cycle;
            }
            for (int i=0;i<ARRAYLEN(ecat_input[slaves]->info);i++)
            {
                // 清除从站输入的数据
                ecat_input[slaves]->info[i] = 0;
            }
        }
        delay = (DELAY + delay_offset);
        // if (config_ok == FALSE) delay += 30*1000; // 处于配置状态，想要放低通信速度
        // 2. 发送并接收数据
        {
            needlf = TRUE;
            ec_send_processdata();
            wkc = ec_receive_processdata(EC_TIMEOUTRET);
            usleep(delay);
        }
        total_cycle++;
        printf ("{cycle_delay:%d[us] totalcycle:%lld} ", delay+delay_offset, total_cycle);
        for (int k=0;k<ARRAYLEN(ecat_input[0]->info);k++)
        {
            printf ("rev[%d]:0x%x, ",k, ecat_input[0]->info[k]);
        }
        printf ("\n");
    }
    inOP = FALSE;
}

 OSAL_THREAD_FUNC ecat_sdo_config(void *ptr)
 {
    // 遍历 所有从站 中的值
    for (int slaves=1;slaves<=ec_slavecount;slaves++)
    {
        printf ("SDO info:\n");
        boolean ca = 0;
        uint32 size = 4;
        uint32 device_type = 0;
        uint32 TIME_OUT = 0xFFFFFFFF;
        /*******************************标准对象（0x1000-0x1FFF）**************************/
        // device type
        ec_SDOread(slaves, 0x1000, 0, ca, &size, &device_type, TIME_OUT);
        printf ("\t设备类型 device type: 0x%X\n", device_type);
        // device name
        char device_name[100];
        size = ARRAYLEN(device_name);
        ec_SDOread(slaves,0x1008,0, ca, &size, device_name, TIME_OUT);
        printf ("\t设备名称 device name: %s\n", device_name);
        // Hardware version
        char dHardware_version[100];
        size = ARRAYLEN(dHardware_version);
        ec_SDOread(slaves,0x1009,0, ca, &size, dHardware_version, TIME_OUT);
        printf ("\t硬件版本 Hardware version: %s\n", dHardware_version);
        // Software version
        char Software_version[100];
        size = ARRAYLEN(Software_version);
        ec_SDOread(slaves,0x100A,0, ca, &size, Software_version, TIME_OUT);
        printf ("\t软件版本 Software version: %s\n", Software_version);
        // Restore default parameters
        uint8 Default=0;
        size = 1;
        ec_SDOread(slaves,0x1011, 0, ca, &size, &Default, TIME_OUT);
        printf ("\t默认参数 default: 0x%X\n", Default);
        uint32 parameters = 0;
        size = 4;
        ec_SDOread(slaves,0x1011, 1, ca, &size, &parameters, TIME_OUT);
        printf ("\t默认参数 parameters: 0x%X\n", parameters);
        // Identity
        uint8 Identity = 0;
        size = 1;
        ec_SDOread(slaves, 0x1018, 0, ca, &size, &Identity, TIME_OUT);
        printf ("\t身份: 0x%X\n", Identity);
        uint32 Vendor_ID = 0;
        size = 4;
        ec_SDOread(slaves, 0x1018, 1, ca, &size, &Vendor_ID, TIME_OUT);
        printf ("\t供应商 ID: 0x%X\n", Vendor_ID);
        uint32 Product_Code = 0;
        size = 4;
        ec_SDOread(slaves, 0x1018, 2, ca, &size, &Product_Code, TIME_OUT);
        printf ("\t产品代码: 0x%X\n", Product_Code);
        uint32 Revision = 0;
        size = 4;
        ec_SDOread(slaves, 0x1018, 3, ca, &size, &Revision, TIME_OUT);
        printf ("\t修订: 0x%X\n", Revision);
        uint32 Serial_number = 0;
        size = 4;
        ec_SDOread(slaves, 0x1018, 4, ca, &size, &Serial_number, TIME_OUT);
        printf ("\t序列号: 0x%X\n", Serial_number);
        //Index 10F8 Actual Time Stamp 时间戳
        uint64 Actual_Time = 0;
        size = 8;
        ec_SDOread(slaves, 0x10F8, 0, ca, &size, &Actual_Time, TIME_OUT);
        printf ("\t时间戳 Actual time: 0x%lX\n", Actual_Time);
        
        // 读取 PDO 配置
        printf ("发送给从站 PDO info:\n");
        // Index 1600-167C RxPDO-Map DP-Slave yyy
        for (uint8 j=0;j<=0x7C;j++)
        {
            // 选择 索引
            uint32 Index = 0x1600+j;
            uint8 SubIndexCnt = 0;
            size = 1;
            ec_SDOread(slaves, Index, 0, ca, &size, &SubIndexCnt, TIME_OUT);
            if (SubIndexCnt>0)
                printf ("\tIndex[0x%X], SubIndexCnt: 0x%X\n\t\t", Index, SubIndexCnt);
            // 查找 子索引 对应的值
            for (uint8 i=0;i<SubIndexCnt;i++)
            {
                if (i>0 && i%8 == 0) printf ("\n\t\t");
                uint32 SubIndex = 0;
                size = 4;
                ec_SDOread(slaves, Index, 1, ca, &size, &SubIndex, TIME_OUT);
                if (SubIndex>0)
                    printf ("[0x%X] ", SubIndex);
            }
            if (SubIndexCnt>0)
                printf ("\n");
        }
        // Index 167F DPM RxPDO-Map Control
        // uint8 DPM_RxPDO_MapControl = 0;
        // size = 1;
        // ec_SDOread(1, 0x167F, 0, ca, &size, &DPM_RxPDO_MapControl, TIME_OUT);
        // printf ("PDO 映射 RxPDO 128: %X\n", DPM_RxPDO_MapControl);
        // uint32 DPM_RxPDO_MapControl_1 = 0;
        // size =4;
        // ec_SDOread(1, 0x167F, 1, ca, &size, &DPM_RxPDO_MapControl_1, TIME_OUT);
        // printf ("\t0x%X\n", DPM_RxPDO_MapControl_1);
        // uint8 DPM_RxPDO_MapControl_2 = 0;
        // size = 4;
        // ec_SDOread(1, 0x167F, 2, ca, &size, &DPM_RxPDO_MapControl_2, TIME_OUT);
        // printf ("\t0x%X\n", DPM_RxPDO_MapControl_2);

        printf ("接收从站 PDO info:\n");
        // Index 1A00-1A7C DPS TxPDO-Map Slave yyy
        for (uint8 j=0;j<=0x7C;j++)
        {
            // 选择 索引
            uint32 Index = 0x1A00+j;
            uint8 SubIndexCnt = 0;
            size = 1;
            ec_SDOread(slaves, Index, 0, ca, &size, &SubIndexCnt, TIME_OUT);
            if (SubIndexCnt>0)
                printf ("\tIndex[0x%X], SubIndexCnt: 0x%X\n\t\t", Index, SubIndexCnt);
            // 查找 子索引 对应的值
            for (uint8 i=0;i<SubIndexCnt;i++)
            {
                if (i>0 && i%8 == 0) printf ("\n\t\t");
                uint32 SubIndex = 0;
                size = 4;
                ec_SDOread(slaves, Index, 1, ca, &size, &SubIndex, TIME_OUT);
                if (SubIndex>0)
                    printf ("[0x%X] ", SubIndex);
            }
            if (SubIndexCnt>0)
                printf ("\n");
        }

        // Index 1C00 Sync manager type 通道1-4
        uint8 Sync_Manager_type = 0;
        size = 1;
        ec_SDOread(slaves, 0x1C00, 0, ca, &size, &Sync_Manager_type, TIME_OUT);
        uint8 Sync_Manager_1 = 0;
        ec_SDOread(slaves, 0x1C00, 1, ca, &size, &Sync_Manager_1, TIME_OUT);
        printf ("Sync-Manager 类型通道 1-邮箱写入: %d\n", Sync_Manager_1);
        uint8 Sync_Manager_2 = 0;
        ec_SDOread(slaves, 0x1C00, 2, ca, &size, &Sync_Manager_type, TIME_OUT);
        printf ("Sync-Manager 类型通道 2-邮箱读取: %d\n", Sync_Manager_2);
        uint8 Sync_Manager_3 = 0;
        ec_SDOread(slaves, 0x1C00, 3, ca, &size, &Sync_Manager_type, TIME_OUT);
        printf ("Sync-Manager 类型通道 3-过程数据写入（输出）: %d\n", Sync_Manager_3);
        uint8 Sync_Manager_4 = 0;
        ec_SDOread(slaves, 0x1C00, 4, ca, &size, &Sync_Manager_type, TIME_OUT);
        printf ("Sync-Manager 类型通道 4-过程数据读取（输入）: %d\n", Sync_Manager_4);

        // Index 1C12 RxPDO assign
        uint8 RxPdo_assign = 0;
        size = 1;
        ec_SDOread(slaves, 0x1C12, 0, ca, &size, &RxPdo_assign, TIME_OUT);
        printf ("RxPdo assign: %d\n", RxPdo_assign);
        // for (uint8 i=1;i<=0x7E;i++)
        // {
        //     if (i>0 && i%8==0) printf ("\n");
        //     int index = 0x1C12+i;
        //     uint32 x1C12 = 0;
        //     ec_SDOread(1, index, i, ca, &size, &x1C12, 0xFFFFFFFF);
        //     printf ("\t%d ", x1C12);
        // }
        // printf ("\n");
        
        // Index 1C13 TxPDO assign
        uint8 TxPdo_assign = 0;
        size = 1;
        ec_SDOread(slaves, 0x1C13, 0, ca, &size, &TxPdo_assign, TIME_OUT);
        printf ("TxPdo assign: %d\n", TxPdo_assign);
        // for (uint8 i=1;i<=0x86;i++)
        // {
        //     if (i>0 && i%8==0) printf ("\n");
        //     int index = 0x1C13+i;
        //     uint32 x1C13 = 0;
        //     ec_SDOread(slaves, index, i, ca, &size, &x1C13, 0xFFFFFFFF);
        //     printf ("\t%d ", x1C13);
        // }
        // printf ("\n");

        // Index 1C32 SM output parameter
        uint8 SM_output_parameter = 0;
        size = 1;
        ec_SDOread(slaves, 0x1C32, 0, ca, &size, &SM_output_parameter, TIME_OUT);
        printf ("输出的同步参数: %d\n", SM_output_parameter);
        // 当前的同步模式
        uint16 Sync_mode = 0;
        size = 2;
        ec_SDOread(slaves, 0x1C32, 1, ca, &size, &Sync_mode, TIME_OUT);
        printf ("当前的同步模式: %d\n", Sync_mode);
        // Cycle time 循环时间
        uint32 Cycle_time = 0;
        size = sizeof(Cycle_time);
        ec_SDOread(slaves, 0x1C32, 2, ca, &size, &Cycle_time, TIME_OUT);
        printf ("循环时间: %d\n", Cycle_time);
        // 从 SYNC0 事件到读取输入的时间（单位：ns，仅 DC 模式）
        uint32 SYNC0_ns = 0;
        size = sizeof(SYNC0_ns);
        ec_SDOread(slaves, 0x1C32, 3, ca, &size, &SYNC0_ns, TIME_OUT);
        printf ("从 SYNC0 事件到读取输入的时间(单位:ns,仅 DC 模式): %d\n", SYNC0_ns);
        // Sync modes supported
        uint16 Sync_modes_supported = 0;
        size = sizeof(Sync_modes_supported);
        ec_SDOread(slaves, 0x1C32, 4, ca, &size, &Sync_modes_supported, TIME_OUT);
        printf ("支持的同步模式: %d\n", Sync_modes_supported);
        if (Sync_modes_supported & 1<<0) printf ("支持 Free Run\n");
        if (Sync_modes_supported & 1<<1) printf ("支持与 SM 2 事件同步（输出可用）\n");
        if (Sync_modes_supported & 1<<2) printf ("支持与 SM 3 事件同步（无输出可用）\n");
        if ((Sync_modes_supported>>2 & 3<<0) == 1) printf ("支持 DC 模式\n");
        if ((Sync_modes_supported>>4 & 3<<0) == 1) printf ("基于本地事件的输入偏移（输出可用）\n");
        if ((Sync_modes_supported>>4 & 3<<0) == 2) printf ("基于 SYNC1 事件的输入偏移（无输出可用）\n");
        if (Sync_modes_supported & 1<<14) printf ("动态周期\n");
        // Minimum cycle time
        uint32 Minimum_cycle_time = 0;
        size = sizeof(Minimum_cycle_time);
        ec_SDOread(slaves, 0x1C32, 5, ca, &size, &Minimum_cycle_time, TIME_OUT);
        printf ("此条目包含 DP 循环的测量周期(单位: ns): %d\n", Minimum_cycle_time);
        // Minimum delay time
        uint32 Minimum_delay_time = 0;
        size = sizeof(Minimum_delay_time);
        ec_SDOread(slaves, 0x1C32, 7, ca, &size, &Minimum_delay_time, TIME_OUT);
        printf ("Minimum_delay_time: %d\n", Minimum_delay_time);
        // Delay time
        // 该条目包含第二个事件（SYNC0 事件（DC-SYNC0 同步）或 SYNC1 事件（DC SYNC1 同步））之后的时间，以及输出的时间和输出量（单位：ns，仅限 DC 模式）
        uint32 Delay_time = 0;
        size = sizeof(Delay_time);
        ec_SDOread(slaves, 0x1C32, 9, ca, &size, &Delay_time, TIME_OUT);
        printf ("Delay_time: %d\n", Delay_time);

        // Index 6000-67C0 DP Inputs Slave yyy
        uint8 DP_Inputs = 0;
        size = 1;
        ec_SDOread(slaves, 0x6000, 0, ca, &size, &DP_Inputs, TIME_OUT);
        printf ("x6000 Subindex count: %d\n", DP_Inputs);
        // Index 7000-77C0 DP Outputs Slave yyy
        uint8 DP_Outputs = 0;
        size = 1;
        ec_SDOread(slaves, 0x7F80, 0, ca, &size, &DP_Outputs, TIME_OUT);
        printf ("x7F80 Subindex count: %d\n", DP_Outputs);

        #if 0
        time_t rawtime;
        struct tm *timeinfo;
        char buffer[20]; // 用于存储格式化后的时间字符串
        struct timespec ts;
        int Tx = 0, Rx = 0, ret=0;
        int subsize = 4, subindexcount=0;
        int index[] = {0x9000, 0x9010, 0x9020, 0x9030, 0x9040, 0x9050, \
            0x9060, 0x9070, 0x9080, 0x9090};
        printf ("index cnt:%ld\n",ARRAYLEN(index));
        // 遍历 index 中的值
        for (int i=0;i<ARRAYLEN(index);i++)
        {
            // 获取当前时间
            time(&rawtime);
            timeinfo = localtime(&rawtime);
            // 格式化时间，格式为 "年月日 时分秒"
            strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
            
            // 获取高精度时间
            if (clock_gettime(CLOCK_REALTIME, &ts) == -1) {
                perror("clock_gettime");
                return ;
            }
            printf(" Current time: %s", buffer);
            printf(" %ld[ms], %ld[ns]\n", ts.tv_nsec/1000, ts.tv_nsec);

            // 获取索引下的子索引数
            subsize = 1;
            ec_SDOread (slaves, index[i], 0, ca, &subsize, &subindexcount, TIME_OUT);
            printf ("index:0x%X, subindexcount:%d\n", index[i], subindexcount);
            // 检查数据
            unsigned char buff[228]; // 最大发送字节为228，在xml文件的邮箱中限制
            for (int j=1;j<=subindexcount;j++)
            {
                subsize = 4;
                Tx = index[i] + j, Rx = 0;
                for (int i=0;i<sizeof(buff);i++) buff[i] = i;
                ec_SDOwrite (slaves, index[i], j, ca, sizeof(buff), &buff, TIME_OUT);
                // ec_SDOwrite (slaves, index[i], j, ca, subsize, &Tx, TIME_OUT);
                // ec_SDOread (slaves, index[i], j, ca, &subsize, &Rx, TIME_OUT);
                // if (Rx != Tx) 
                // {
                //     printf ("\t[0x%X] subindex[0x%X], Tx=0x%X, Rx=0x%X\n",index[i], j, Tx, Rx);
                //     ret++;
                // }
            }
            if (ret == 0) printf ("\tok,total:%ldByte!\n", subindexcount*sizeof(buff));
            else printf ("\tcheck sdo data error! ret=%d\n", ret);
            ret=0;
            time(&rawtime);
            timeinfo = localtime(&rawtime);
            // 格式化时间，格式为 "年月日 时分秒"
            strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
            
            // 获取高精度时间
            if (clock_gettime(CLOCK_REALTIME, &ts) == -1) {
                perror("clock_gettime");
                return ;
            }
            printf(" Current time: %s", buffer);
            printf(" %ld[ms], %ld[ns]\n", ts.tv_nsec/1000, ts.tv_nsec);
        }
        #endif
    }
    config_ok = FALSE;
 }

 OSAL_THREAD_FUNC ecatcheck( void *ptr )
 {
     int slave;
     (void)ptr;                  /* Not used */
    //  return ;
     while(ecat_signal==FALSE && config_ok == TRUE)
     {
         if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
         {
             if (needlf)
             {
                needlf = FALSE;
                printf("\n");
             }
             /* one ore more slaves are not responding */
             ec_group[currentgroup].docheckstate = FALSE;
             ec_readstate();
             for (slave = 1; slave <= ec_slavecount; slave++)
             {
                 if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
                 {
                     ec_group[currentgroup].docheckstate = TRUE;
                     if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
                     {
                         printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
                         ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
                         ec_writestate(slave);
                     }
                     else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
                     {
                         printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
                         ec_slave[slave].state = EC_STATE_OPERATIONAL;
                         ec_writestate(slave);
                     }
                     else if(ec_slave[slave].state > EC_STATE_NONE)
                     {
                         if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
                         {
                             ec_slave[slave].islost = FALSE;
                             printf("MESSAGE : slave %d reconfigured\n",slave);
                         }
                     }
                     else if(!ec_slave[slave].islost)
                     {
                         /* re-check state */
                         ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
                         if (ec_slave[slave].state == EC_STATE_NONE)
                         {
                             ec_slave[slave].islost = TRUE;
                             printf("ERROR : slave %d lost\n",slave);
                         }
                     }
                }
                if (ec_slave[slave].islost)
                {
                   if(ec_slave[slave].state == EC_STATE_NONE)
                   {
                     if (ec_recover_slave(slave, EC_TIMEOUTMON))
                     {
                     ec_slave[slave].islost = FALSE;
                     printf("MESSAGE : slave %d recovered\n",slave);
                     }
                   }
                   else
                   {
                     ec_slave[slave].islost = FALSE;
                     printf("MESSAGE : slave %d found\n",slave);
                   }
                }
             }
             if(!ec_group[currentgroup].docheckstate)
             {
                 time_t rawtime;
                 struct tm *timeinfo;
                 char buffer[20]; // 用于存储格式化后的时间字符串
                 // 获取当前时间
                 time(&rawtime);
                 timeinfo = localtime(&rawtime);
                 // 格式化时间，格式为 "年月日 时分秒"
                 strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
                 
                 struct timespec ts;
                 // 获取高精度时间
                 if (clock_gettime(CLOCK_REALTIME, &ts) == -1) {
                     perror("clock_gettime");
                     return ;
                 }
                 printf("OK : all slaves resumed OPERATIONAL.");
                 printf (" {cycle_delay:%d[us] totalcycle:%lld}", delay+delay_offset, total_cycle);
                 printf(" Current time: %s", buffer);
                 printf(" %ld[ms], %ld[ns]\n", ts.tv_nsec/1000, ts.tv_nsec);
                 // delay_offset = 0;
             }
         }
         osal_usleep(10000);
     }
 }
 
 void ecat_init(char *ifname)
 {
     int i, j, oloop, iloop, chk = 0, dc=0;
     needlf = FALSE;
     inOP = FALSE;
 
    printf("Starting simple test\n");
 
    /* initialise SOEM, bind socket to ifname */
    if (!ec_init(ifname))
    {
        printf("No socket connection on %s\nExcecute as root\n",ifname);
        return ;
    }
    printf("ec_init on %s succeeded.\n",ifname);
 
    /* find and auto-config slaves */
    if ( ec_config_init(FALSE) <= 0 )
    {
        printf("No slaves found!\n");
        goto exit_t;
    }
    printf("%d slaves found and configured.\n",ec_slavecount);

    int iomap_size = ec_config_map(&IOmap);
    printf ("iomap size: %d\n", iomap_size);
    
    #if (DC_ENABLE)
    dc = ec_configdc();
    #endif

    printf("Slaves mapped, state to SAFE_OP.\n");
    /* wait for all slaves to reach SAFE_OP state */
    ec_statecheck(1, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);

    printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);

    expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
    printf("Calculated workcounter %d\n", expectedWKC);

    ec_slave[0].state = EC_STATE_OPERATIONAL;
    /* send one valid process data to make outputs in slaves happy*/
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    /* request OP state for all slaves */
    ec_writestate(0);

    /* wait for all slaves to reach OP state */
    for (int slave=0;slave<=ec_slavecount;slave++)
    {
        ec_slave[slave].state = EC_STATE_OPERATIONAL;
        /* send one valid process data to make outputs in slaves happy*/
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
        /* request OP state for all slaves */
        ec_writestate(slave);
        do 
        {
            ec_send_processdata();
            ec_receive_processdata(EC_TIMEOUTRET);
            ec_statecheck(slave, EC_STATE_OPERATIONAL, 1000);
            osal_usleep(10);
            printf ("slave%d ec_statecheck chk: %d\r",slave,  chk++);
            if (ecat_signal)
                goto exit_t;
        }while(ec_slave[slave].state != EC_STATE_OPERATIONAL);
        printf ("\n");
    }
    
    if(dc > 0)
    {
        boolean dc_enable = FALSE;
        printf ("dc enable\n");
        for (int slaves=1;slaves<=ec_slavecount;slaves++)
        {
            if (ec_slave[slaves].hasdc && dc_enable == FALSE)
            {
                dc_enable = TRUE;
                ec_dcsync0(slaves, TRUE, DC_SYNC0, 0);// 时间同步触发时间 ns
                ec_dcsync01(slaves, TRUE, DC_SYNC0, DC_SYNC1, 0);// 设置偏移时间
            }
            printf ("Slave%d:", slaves);
            printf (" Hasdc:%d, DCactive:%d ", ec_slave[slaves].hasdc, ec_slave[slaves].DCactive);
            printf (" DCrtA:%d , DCrtB:%d ", ec_slave[slaves].DCrtA, ec_slave[slaves].DCrtB);
            printf (" Pdelay:%d[ns], DCcyclens:%d[ns] DCshift:%d\n", ec_slave[slaves].pdelay, ec_slave[slaves].DCcycle, ec_slave[slaves].DCshift);
        }
    }
    else 
        printf ("dc disable\n");

    printf("Operational state reached for all slaves.\n");
    for (int slaves=1;slaves<=ec_slavecount;slaves++)
    {
        if (ec_slave[slaves].state == EC_STATE_OPERATIONAL )
        {
            ecat_output[slaves-1] = (output_t *)ec_slave[slaves].outputs;
            ecat_input[slaves-1] = (input_t *)ec_slave[slaves].inputs;
        }
        else
        {
            printf("Not all slaves reached operational state.\n");
            ec_readstate();
            for(i = 1; i<=ec_slavecount ; i++)
            {
                if(ec_slave[i].state != EC_STATE_OPERATIONAL)
                {
                    printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                        i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                }
            }
        }
    }
    // SDO输出
    ecat_sdo_config(NULL);
    // 开启从站检测
    osal_thread_create(&osal_ecatcheck_thread, 128000, &ecatcheck, (void*) &ctime);
    // 开启PDO数据循环
    osal_thread_create(&osal_ecatdata_thread, 128000, ecat_data, (void*) &ctime);
    // osal_thread_create(&osal_ecattime_thread, 128000, ecat_sdo_config, (void*) &ctime);
    inOP = TRUE;
    while (inOP && ecat_signal==FALSE)
    {
        osal_usleep(2*1000);
    }

exit_t:
    printf("\nRequest init state for all slaves\n");
    ec_slave[0].state = EC_STATE_INIT;
    /* request INIT state for all slaves */
    ec_writestate(0);

    printf("End ecather soem, close socket\n");
    /* stop SOEM, close socket */
    ec_close();
}

void sighandler_cb(int sig)
{
    printf ("sig: SIGINT\n");

    ecat_signal = TRUE;
    osal_usleep(200*1000);
}

int main(int argc, char *argv[])
{

    time_t rawtime;
    struct tm *timeinfo;
    char buffer[20]; // 用于存储格式化后的时间字符串
    // 获取当前时间
    time(&rawtime);
    timeinfo = localtime(&rawtime);
    // 格式化时间，格式为 "年月日 时分秒"
    strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
    printf ("开始测试 :%s\n", buffer);

    signal(SIGINT, sighandler_cb);

    printf("SOEM (Simple Open EtherCAT Master)\n");

    if (argc > 1)
    {
        ecat_init(argv[1]);}
    else
    {
        printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
    }

    // 获取当前时间
    time(&rawtime);
    timeinfo = localtime(&rawtime);
    // 格式化时间，格式为 "年月日 时分秒"
    strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
    printf ("结束测试 :%s\n", buffer);
    printf("End main\n");
    return (0);
}
 
